{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "请参考博客： https://blog.csdn.net/weixin_42301220/article/details/124836339\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "class LateralErrorModel:\n",
    "    \"\"\"\n",
    "    This model describes the lateral motion of a vehicle and its response to various inputs.\n",
    "\n",
    "    Parameters:\n",
    "        self.m (float): Mass of the vehicle (kg).\n",
    "        self.Vx (float): Longitudinal velocity (self.m/s).\n",
    "        self.C_alpha_f (float): Front tire cornering stiffness (N/rad).\n",
    "        self.C_alpha_r (float): Rear tire cornering stiffness (N/rad).\n",
    "        self.l_f (float): Distance from the vehicle's center of mass to the front axle (self.m).\n",
    "        self.l_r (float): Distance from the vehicle's center of mass to the rear axle (self.m).\n",
    "        self.I_z (float): Moment of inertia about the vertical axis (kg self.m^2).\n",
    "        self.g (float): Acceleration due to gravity (self.m/s^2).\n",
    "    \"\"\"\n",
    "    def __init__(self, m, Vx, C_alpha_f, C_alpha_r, l_f, l_r, I_z, g):\n",
    "        self.m = m\n",
    "        self.Vx = Vx\n",
    "        self.C_alpha_f = C_alpha_f\n",
    "        self.C_alpha_r = C_alpha_r\n",
    "        self.l_f = l_f\n",
    "        self.l_r = l_r\n",
    "        self.I_z = I_z\n",
    "        self.g = g\n",
    "        \n",
    "    def GenerateStateSpace(self):\n",
    "        A = np.array([[0, 1, 0, 0],\n",
    "                           [0, -(2*self.C_alpha_f + 2*self.C_alpha_r) / (self.m * self.Vx), (2*self.C_alpha_f + 2*self.C_alpha_r) / self.m,\n",
    "                            (-2*self.C_alpha_f*self.l_f + 2*self.C_alpha_r*self.l_r) / (self.m * self.Vx)],\n",
    "                           [0, 0, 0, 1],\n",
    "                           [0, -(2*self.l_f*self.C_alpha_f - 2*self.l_r*self.C_alpha_r) / (self.I_z * self.Vx),\n",
    "                            (2*self.l_f*self.C_alpha_f - 2*self.l_r*self.C_alpha_r) / self.I_z,\n",
    "                            (-2*self.l_f**2*self.C_alpha_f + 2*self.l_r**2*self.C_alpha_r) / (self.I_z * self.Vx)]])\n",
    "        \n",
    "        B = np.array([[0],\n",
    "                           [2 * self.C_alpha_f / self.m],\n",
    "                           [0],\n",
    "                           [2 * self.l_f * self.C_alpha_f / self.I_z]])\n",
    "        \n",
    "        C = np.array([[0],\n",
    "                           [(-2*self.C_alpha_f*self.l_f + 2*self.C_alpha_r*self.l_r) / (self.m * self.Vx) - self.Vx],\n",
    "                           [0],\n",
    "                           [(-2*self.l_f**2*self.C_alpha_f + 2*self.l_r**2*self.C_alpha_r) / (self.I_z * self.Vx)]])\n",
    "        \n",
    "        D = np.array([[0],\n",
    "                           [self.g],\n",
    "                           [0],\n",
    "                           [0]])\n",
    "        return A,B,C,D\n",
    "    def ComputeStateDerivative(self, state, delta, psi_des, phi):\n",
    "        \"\"\"\n",
    "        Computes the derivative of the state vector using the lateral error dynamics model.\n",
    "\n",
    "        Parameters:\n",
    "            state (numpy.ndarray): The current state vector [e_y, e_y_dot, e_psi, e_psi_dot].\n",
    "            delta (float): The steering input.\n",
    "            psi_des (float): The desired yaw rate input.\n",
    "            phi (float): The roll angle input.\n",
    "\n",
    "        Returns:\n",
    "            numpy.ndarray: The derivative of the state vector [e_y_dot, e_y_ddot, e_psi_dot, e_psi_ddot].\n",
    "        \"\"\"\n",
    "        A,B,C,D=self.GenerateStateSpace()\n",
    "        state_dot = np.dot(A, state) + np.dot(B, delta) + np.dot(C, psi_des) + np.dot(D, np.sin(phi))\n",
    "        return state_dot[:,0]\n",
    "    def DiscreteStateSpace(self,dt):\n",
    "        \"\"\"\n",
    "        Returns:\n",
    "            discrete state space\n",
    "        \"\"\"\n",
    "        A,B,C,D = self.GenerateStateSpace()\n",
    "        I = np.eye(4)\n",
    "        A_bar = I + A * dt\n",
    "        B_bar = B*dt\n",
    "        return A_bar,B_bar\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "State derivative: [0.         2.97836748 0.         1.        ]\n",
      "A_bar:  [[1.    0.1   0.    0.   ]\n",
      " [0.    0.75  5.    0.   ]\n",
      " [0.    0.    1.    0.1  ]\n",
      " [0.    0.    0.    0.975]]\n",
      "B_bar:  [[0.]\n",
      " [2.]\n",
      " [0.]\n",
      " [1.]]\n"
     ]
    }
   ],
   "source": [
    "model = LateralErrorModel(m=1000.0, Vx=20.0, C_alpha_f=10000.0, C_alpha_r=15000.0, l_f=1.5, l_r=1.0, I_z=3000.0, g=9.8)\n",
    "\n",
    "# Initial conditions and inputs\n",
    "initial_state = np.array([0.0, 0.0, 0.0, 0.0])\n",
    "delta_input = 0.1\n",
    "psi_des_input = 0.0\n",
    "phi_input = 0.1\n",
    "dt = 0.1\n",
    "# Compute the derivative of the state using the model instance\n",
    "state_derivative = model.ComputeStateDerivative(initial_state, delta_input, psi_des_input, phi_input)\n",
    "A_bar, B_bar = model.DiscreteStateSpace(dt)\n",
    "print(\"State derivative:\", state_derivative)\n",
    "print(\"A_bar: \", A_bar)\n",
    "print(\"B_bar: \", B_bar)\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "tftorch",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.16"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
